import serial
import time
import pynmea2  # 用于解析NMEA语句
from pyproj import Transformer, CRS  # 用于坐标转换
import math

# 串口参数 (根据您的 RTK 接收器进行修改)
port = "/dev/ttyUSB0"
baudrate = 115200
bytesize = serial.EIGHTBITS
parity = serial.PARITY_NONE
stopbits = serial.STOPBITS_ONE
timeout = 1

# WGS84坐标系 (EPSG:4326)
wgs84 = CRS("EPSG:4326")

# 假设你所在的区域是UTM zone 49N (例如广州), 请根据实际情况修改
utm_zone = CRS("EPSG:32649")  # WGS 84 / UTM zone 49N

# 创建坐标转换器
transformer = Transformer.from_crs(wgs84, utm_zone)

# 上一个点的UTM坐标 (初始化为None)
previous_utm_x = None
previous_utm_y = None

# 累计距离 (初始化为0)
total_distance = 0.0


def calculate_distance(x1, y1, x2, y2):
    """计算两个点之间的欧氏距离"""
    return math.sqrt((x2 - x1)**2 + (y2 - y1)**2)

try:
    # 打开串口
    ser = serial.Serial(port, baudrate, bytesize, parity, stopbits, timeout)
    print(f"串口 {port} 已打开")

    # 读取数据
    while True:
        if ser.in_waiting > 0:
            line = ser.readline()
            try:
                decoded_line = line.decode('utf-8').strip()
                # print(decoded_line) # 可以选择不打印原始数据

                # 解析NMEA语句
                try:
                    msg = pynmea2.parse(decoded_line)

                    # 处理GGA消息
                    if isinstance(msg, pynmea2.GGA) and msg.gps_qual >= 1:  # 确保有定位数据
                        # 转换为UTM坐标
                        utm_x, utm_y = transformer.transform(msg.latitude, msg.longitude)

                        # 如果有上一个点的数据，计算距离
                        if previous_utm_x is not None and previous_utm_y is not None:
                            distance = calculate_distance(previous_utm_x, previous_utm_y, utm_x, utm_y)
                            total_distance += distance
                            print(f"Time: {msg.timestamp}, Lat: {msg.latitude:.6f}, Lon: {msg.longitude:.6f}, "
                                  f"UTM X: {utm_x:.3f}, UTM Y: {utm_y:.3f}, "
                                  f"Distance from previous: {distance:.3f} m, Total distance: {total_distance:.3f} m")
                        # 更新上一个点的数据
                        previous_utm_x, previous_utm_y = utm_x, utm_y


                except pynmea2.ParseError as e:
                    print(f"NMEA解析错误: {e}")
                except Exception as e:
                    print(f"处理数据时发生错误: {e}")


            except UnicodeDecodeError:
                print(f"解码错误, 原始数据: {line}")

        time.sleep(0.01)  # 降低采样率

except serial.SerialException as e:
    print(f"串口错误：{e}")
except KeyboardInterrupt:
    print("程序已停止")
finally:
    if ser.is_open:
        ser.close()
        print("串口已关闭")